#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include "localization_msgs/localization_msg.h"
#include "control_msgs/control_msg.h"

#include "actuator_msgs/actuator_msg.h"
#include "control_msgs/control_state_msg.h"
#include "common_lib/common.h"
#include "amr_trajectory_msgs/amr_trajectory_msg.h"

visualization_msgs::MarkerArray tra_points;
visualization_msgs::MarkerArray tra_pure_points;
visualization_msgs::MarkerArray tra_all_points;
visualization_msgs::Marker car_pose;
visualization_msgs::Marker work_rotate_pose;
visualization_msgs::Marker control_pose;
visualization_msgs::Marker lift_height_text;
visualization_msgs::Marker name_text;
control_msgs::control_state_msg control_state_;
// 上装状态 <上装类型, 上装状态>
//  std::map<uint8_t, std::map<uint8_t, actuator_msgs::WorkStateMsg> > all_work_state;

// 定位消息
localization_msgs::localization_msg curr_localiation;

common_lib::Common common_cal_;

void TrajectoryAllCallback(const amr_trajectory_msgs::amr_trajectory_msg &msg)
{
    visualization_msgs::Marker point;
    tra_all_points.markers.clear();
    int i = 0;
    for (auto p : msg.points)
    {
        point.header.frame_id = "global";
        point.type = visualization_msgs::Marker::CUBE;
        point.header.stamp = ros::Time::now();
        point.lifetime = ros::Duration();
        point.frame_locked = true;
        point.action = visualization_msgs::Marker::ADD;
        point.frame_locked = true;
        point.ns = "";
        point.id = i++;

        point.pose = p.pose;
        if (p.move_direction == "forward")
            point.pose.orientation.z = 1;
        else
            point.pose.orientation.z = 0;

        point.scale.x = 0.05;
        point.scale.y = 0.05;
        point.scale.z = 0.1;

        point.color.r = 1.0f;
        point.color.g = 0.0f;
        point.color.b = 0.0f;
        point.color.a = 1.0;

        if (i == msg.points.size())
        {
            point.color.r = 1.0f;
            point.color.g = 0.0f ;
            point.color.b = 0.0f;
            point.color.a = 1.0;
            point.scale.x = 0.3;
            point.scale.y = 0.3;
            point.scale.z = 0.1;
        }

        tra_all_points.markers.push_back(point);
    }
    // all_trajectory_pub.publish(tra_all_points);
}

void TrajectoryCallback(const amr_trajectory_msgs::amr_trajectory_msg &msg)
{
    visualization_msgs::Marker point;
    tra_points.markers.clear();
    int i = 0;
    for (auto p : msg.points)
    {
        point.header.frame_id = "global";
        point.type = visualization_msgs::Marker::CUBE;
        point.header.stamp = ros::Time::now();
        point.lifetime = ros::Duration();
        point.frame_locked = true;
        point.action = visualization_msgs::Marker::ADD;
        point.frame_locked = true;
        point.ns = "";
        point.id = i++;
        // point.pose.position.x = p.pose2D.x;
        // point.pose.position.y = p.pose2D.y;
        // point.pose.position.z = p.pose2D.theta;
        // point.pose.orientation.x = 0;
        // point.pose.orientation.y = 0;
        // if (p.move_direction == "forward")
        //     point.pose.orientation.z = 1;
        // else
        //     point.pose.orientation.z = 0;
        // point.pose.orientation.w = p.point_type;
        point.pose = p.pose;
        point.scale.x = 0.1;
        point.scale.y = 0.1;
        point.scale.z = 0.1;
        // p.target_speed = 0.5;
        if (fabs(p.target_speed) < 0.001) // 0
        {
            point.color.r = 1.0f;
            point.color.g = 1.0f;
            point.color.b = 1.0f;
            point.color.a = 1.0;
        }
        else if (fabs(p.target_speed) < 0.5) // 0-1
        {
            point.color.r = 0.0f;
            point.color.g = 1.0f;
            point.color.b = 1.0f - p.target_speed;
            point.color.a = 1.0;
        }
        else
        {
            if (fabs(p.target_speed) > 1) //>2
            {
                point.color.r = 1.0f;
                point.color.g = 0.0f;
                point.color.b = 0.0f;
                point.color.a = 1.0;
            }
            else // 1-2
            {
                point.color.r = p.target_speed - 0.5;
                point.color.g = 1.0f - p.target_speed;
                point.color.b = 0.0f;
                point.color.a = 1.0;
            }
        }
        if (i == msg.points.size())
        {
            point.color.r = 1.0f;
            point.color.g = 0.0f;
            point.color.b = 0.0f;
            point.color.a = 1.0;
            point.scale.x = 0.3;
            point.scale.y = 0.3;
            point.scale.z = 0.1;
        }
        tra_points.markers.push_back(point);
    }

    printf("\033[0m\033[1;31;42m%s size: %d\033[0m\n", __FUNCTION__, (int)msg.points.size());
}
void TrajectoryPureCallback(const amr_trajectory_msgs::amr_trajectory_msg &msg)
{
    visualization_msgs::Marker point;
    tra_pure_points.markers.clear();
    int i = 0;
    for (auto p : msg.points)
    {
        point.header.frame_id = "global";
        point.type = visualization_msgs::Marker::CUBE;
        point.header.stamp = ros::Time::now();
        point.lifetime = ros::Duration();
        point.frame_locked = true;
        point.action = visualization_msgs::Marker::ADD;
        point.frame_locked = true;
        point.ns = "";
        point.id = i++;
        // point.pose.position.x = p.pose2D.x;
        // point.pose.position.y = p.pose2D.y;
        // point.pose.position.z = p.pose2D.theta;
        // if (p.angle_type == "turn")
        //     point.pose.orientation.x = 0;
        // else
        // {
        //     point.pose.orientation.x = 1;
        // }

        // point.pose.orientation.y = p.angle;
        // if (p.direction == "forward")
        //     point.pose.orientation.z = 1;
        // else
        //     point.pose.orientation.z = 0;
        // point.pose.orientation.w = p.point_type;
        point.pose = p.pose;
        point.scale.x = 0.05;
        point.scale.y = 0.05;
        point.scale.z = 0.1;
        // p.target_speed = 0.5;
        if (fabs(p.target_speed) < 0.001) // 0
        {
            point.color.r = 1.0f;
            point.color.g = 1.0f;
            point.color.b = 1.0f;
            point.color.a = 1.0;
        }
        else if (fabs(p.target_speed) < 0.5) // 0-1
        {
            point.color.r = 0.0f;
            point.color.g = 1.0f;
            point.color.b = 1.0f - p.target_speed;
            point.color.a = 1.0;
        }
        else
        {
            if (fabs(p.target_speed) > 1) //>2
            {
                point.color.r = 1.0f;
                point.color.g = 0.0f;
                point.color.b = 0.0f;
                point.color.a = 1.0;
            }
            else // 1-2
            {
                point.color.r = p.target_speed - 0.5;
                point.color.g = 1.0f - p.target_speed;
                point.color.b = 0.0f;
                point.color.a = 1.0;
            }
        }

        tra_pure_points.markers.push_back(point);
    }
}
void LocalizationCallback(const localization_msgs::localization_msg &msg)
{
    curr_localiation = msg;

    car_pose.header.frame_id = "global";
    car_pose.type = visualization_msgs::Marker::ARROW;
    car_pose.header.stamp = ros::Time::now();
    car_pose.lifetime = ros::Duration(10.0f);
    car_pose.frame_locked = true;
    car_pose.action = visualization_msgs::Marker::ADD;
    car_pose.ns = "";
    car_pose.id = 1;
    car_pose.pose = msg.pose;

    car_pose.scale.x = 1;
    car_pose.scale.y = 0.2;
    car_pose.scale.z = 0.2;
    car_pose.color.r = 0.0f;
    car_pose.color.g = 1.0f;
    car_pose.color.b = 0.0f;
    car_pose.color.a = 0.5;
}

void ControlCallback(const control_msgs::control_msg &msg)
{
    control_pose.header.frame_id = "global";
    control_pose.type = visualization_msgs::Marker::CUBE;
    control_pose.header.stamp = ros::Time::now();
    control_pose.lifetime = ros::Duration();
    control_pose.frame_locked = true;
    control_pose.action = visualization_msgs::Marker::ADD;
    control_pose.ns = "";
    control_pose.id = 1;
    // control_pose.pose.position.x = msg.pre_x;
    // control_pose.pose.position.y = msg.pre_y;
    // control_pose.pose.position.z = 0;
    // control_pose.pose.orientation.x = 0.0;
    // control_pose.pose.orientation.y = 0.0;
    // control_pose.pose.orientation.z = 0.0;
    // control_pose.pose.orientation.w = 1.0;

    // control_pose.pose = msg.

    control_pose.scale.x = 1;
    control_pose.scale.y = 1;
    control_pose.scale.z = 0.1;
    control_pose.color.r = 0.0f;
    control_pose.color.g = 1.0f;
    control_pose.color.b = 0.0f;
    control_pose.color.a = 1.0;
}
void NameSet()
{
    name_text.header.frame_id = "global";
    name_text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
    name_text.header.stamp = ros::Time::now();
    name_text.lifetime = ros::Duration();
    name_text.frame_locked = true;
    name_text.action = visualization_msgs::Marker::ADD;
    name_text.ns = "";
    name_text.id = 1;
    name_text.text = "AGV Unmanned System";
    name_text.pose.position.x = 10;
    name_text.pose.position.y = 10;
    name_text.pose.position.z = 0;
    name_text.pose.orientation.x = 0.0;
    name_text.pose.orientation.y = 0.0;
    name_text.pose.orientation.z = 0.0;
    name_text.pose.orientation.w = 1;

    name_text.scale.x = 10;
    name_text.scale.y = 10;
    name_text.scale.z = 2;
    name_text.color.r = 1.0f;
    name_text.color.g = 0.0f;
    name_text.color.b = 0.0f;
    name_text.color.a = 1.0;
}

int main(int argc, char *argv[])
{
    ros::init(argc, argv, "rviz_display");
    ros::NodeHandle n;

    ros::Subscriber sub_trajectory_;
    ros::Subscriber sub_trajectory_pure_;
    ros::Subscriber sub_local_;

    ros::Subscriber sub_control_;
    ros::Subscriber sub_control_state_;
    ros::Subscriber sub_trajectory_all_;
    ros::Subscriber sub_actuatory_;

    sub_trajectory_all_ = n.subscribe("/nav_path", 10, &TrajectoryAllCallback);
    sub_trajectory_ = n.subscribe("/trajectory", 10, &TrajectoryCallback);
    sub_trajectory_pure_ = n.subscribe("/trajectory_pro", 10, &TrajectoryPureCallback);
    sub_local_ = n.subscribe("/localization", 10, &LocalizationCallback);
    sub_control_= n.subscribe("/control", 10, &ControlCallback);

    ros::Publisher all_trajectory_pub = n.advertise<visualization_msgs::MarkerArray>("/rviz_trajectory_all", 100);
    ros::Publisher trajectory_pub = n.advertise<visualization_msgs::MarkerArray>("/rviz_trajectory", 100);
    ros::Publisher trajectory_pure_pub = n.advertise<visualization_msgs::MarkerArray>("/rviz_trajectory_pure", 100);
    ros::Publisher localization_pub = n.advertise<visualization_msgs::Marker>("/rviz_pose", 100);
    // ros::Publisher control_pub = n.advertise<visualization_msgs::Marker>("/rviz_precontrol", 100);
    // ros::Publisher text_pub = n.advertise<visualization_msgs::Marker>("/rviz_work", 100);
    // ros::Publisher name_pub = n.advertise<visualization_msgs::Marker>("/rviz_name", 100);

    car_pose.type = visualization_msgs::Marker::ARROW;
    car_pose.header.stamp = ros::Time::now();
    car_pose.lifetime = ros::Duration();
    car_pose.frame_locked = true;
    car_pose.action = visualization_msgs::Marker::ADD;
    car_pose.ns = "";
    car_pose.id = 1;
    car_pose.header.frame_id = "global";
    car_pose.pose.position.x = 0;
    car_pose.pose.position.y = 0;
    car_pose.pose.position.z = 0;
    car_pose.pose.orientation.x = 0.0;
    car_pose.pose.orientation.y = 0.0;
    car_pose.pose.orientation.z = 0.0;
    car_pose.pose.orientation.w = 1.0;

    car_pose.scale.x = 1;
    car_pose.scale.y = 0.2;
    car_pose.scale.z = 0.2;
    car_pose.color.r = 1.0f;
    car_pose.color.g = 0.0f;
    car_pose.color.b = 0.0f;
    car_pose.color.a = 0.5;

    ros::Rate rate(5);
    while (ros::ok())
    {
        // car_pose.header.stamp = ros::Time::now();
        trajectory_pub.publish(tra_points);
        trajectory_pure_pub.publish(tra_pure_points);
        if (all_trajectory_pub.getNumSubscribers() > 0)
            all_trajectory_pub.publish(tra_all_points);
        localization_pub.publish(car_pose);
        ros::spinOnce();
        rate.sleep();
    }
    ros::shutdown();
    return 0;
}